import math
import numpy as np

PI = math.pi

def Sgn(num):
    if num < 0:
        return -1
    else:
        return 1

def Normalize(v):
    norm = np.linalg.norm(v, ord=1)
    if norm == 0:
        norm = np.finfo(v.dtype).eps
    return v / norm


def MakeTranslation(x,y,z):
    pass



def MakeRotationX(alpha):
    a = alpha * PI / 180.0;
    c = math.cos(a);
    s = math.sin(a);
    #
    m = np.zeros((4, 4))
    m[0][0] = 1.0;
    m[1][1] = m[2][2] = c;
    m[1][2] = s;
    m[2][1] = -s;
    m[3][3] = 1.0;
    #
    return m


def MakeRotationY(alpha):
    a = alpha * PI / 180.0;
    c = math.cos(a);
    s = math.sin(a);
    #
    m = np.zeros(4, 4)
    m[1][1] = 1.0;
    m[0][0] = m[2][2] = c;
    m[0][2] = -s;
    m[2][0] = s;
    m[3][3] = 1.0;
    #
    return m


def MakeRotationZ(alpha):
    a = alpha * PI / 180.0;
    c = math.cos(a);
    s = math.sin(a);
    #
    m = np.zeros((4, 4))
    m[2][2] = 1.0;
    m[0][0] = m[1][1] = c;
    m[0][1] = s;
    m[1][0] = -s;
    m[3][3] = 1.0;
    #
    return m

def Sqr(value):
    return value * value

def MakeRotation(angle, axis):
    if math.fabs(axis[0]) == 0 and math.fabs(axis[1]) == 0:
        return MakeRotationZ(Sgn(axis[2]) * angle)
    #
    axis = Normalize(axis)

    #
    a = angle * PI / 180.0;
    c = math.cos(a);
    s = math.sin(a);
    D = Sqr(axis[0]) + Sqr(axis[1]);
    u0u1 = axis[0]*axis[1];
    u0u2 = axis[0]*axis[2];
    u1u2 = axis[1]*axis[2];
    N = c*u0u1*(Sqr(axis[2])-1)/D;

    #
    m = np.zeros((4, 4))
    m[0][0] = c*(Sqr(axis[1]) + Sqr(u0u2))/D + Sqr(axis[0]);
    m[0][1] = N + s*axis[2] + u0u1;
    m[0][2] = -axis[1]*s + u0u2*(1-c);
    m[0][3] = 0.0;
    #
    m[1][0] = N - s*axis[2] + u0u1;
    m[1][1] = c*(Sqr(axis[0]) + Sqr(u1u2))/D + Sqr(axis[1]);
    m[1][2] = axis[0]*s + u1u2*(1-c);
    m[1][3] = 0.0;
    #
    m[2][0] = axis[1]*s + u0u2*(1-c);
    m[2][1] = -axis[0]*s + u1u2*(1-c);
    m[2][2] = c*D + Sqr(axis[2]);
    m[2][3] = 0.0
    #
    m[3][0] = m[3][1] = m[3][2] = 0.0;
    m[3][3]= 1.0;
    #
    return m


# 等效于 np.dot(v,m)
def Multiply(vector, matrix):

    if len(vector) == 4:
        ret = np.dot(vector, matrix)

    if len(vector) == 3:
        vector.append(0)
        ret = np.dot(vector, matrix)
        ret = ret[:3]

    return ret.tolist()


    x = vector[0]
    y = vector[1]
    z = vector[2]
    #
    newX = matrix[0][0] * x + matrix[1][0] * y + matrix[2][0] * z
    newY = matrix[0][1] * x + matrix[1][1] * y + matrix[2][1] * z
    newZ = matrix[0][2] * x + matrix[1][2] * y + matrix[2][2] * z
    #
    return [newX,newY,newZ]
